#include "imtPublic.h"
#include "imtPosOrientController.h"
#include "imtActionExecuter.h"

namespace iMate
{
	PositionController::PositionController(ActionExecuter* parent,const Vector3& initPos)
		: mParent(parent)
		, mfLength(0.0f)
		, mfSpeed(0.0f)
		, mDistance(0.0f)
		, mbFinish(true)
		, mPos(initPos)
		, mDir(0.0f,0.0f,0.0f)
	{
	}

	const Vector3& PositionController::getCurPos() const
	{
		return mPos;
	}

	void PositionController::onFinish()
	{
		mbFinish = true;
		mfLength = 0.0f;
		mfSpeed = 0.0f;
		mDistance = 0.0f;
		mfCurtime = 0.0f;
		mParent->notifyFinish();
	}

	void PositionController::update(Real timedelta)
	{
		if( mbFinish )	return;

		mfCurtime += timedelta;
		mPos += ( timedelta * mfSpeed ) * mDir;
		mDistance -= mfSpeed * timedelta;

		if( mfCurtime >= mfLength && fabs(mDistance) < 0.5f )
			onFinish();
	}

	void PositionController::setFinished(bool bFinish)
	{
		mbFinish = bFinish;
		if( mbFinish ) onFinish();
	}

	void PositionController::setSpeed(Ogre::Real fspeed)
	{
		this->mfSpeed = fspeed;
	}

	void PositionController::setSrcAndDesPos(const Vector3& src,const Vector3& target)
	{
		mDir = target - src;
		mPos = src;
		mDistance = mDir.length();
		mDir.normalise();
	}

	void PositionController::onStart()
	{
		mbFinish = false;	
		mfLength = mDistance / mfSpeed;
		mfCurtime = 0.0f;
		if( mfLength < 0.01f )
			onFinish();
	}

////////////////////////////////////////////////////////////////////////////////////////////
	OrientationController::OrientationController(ActionExecuter* parent,const Quaternion& initOrient)
		: mParent(parent)
		, mfLength(0.0f)
		, mbFinish(true)
		, mfCurtime(0.0f)
		, mSrcOrient(initOrient)
		, mTargetOrient(initOrient)
	{
	}

	const Quaternion OrientationController::getCurOrient() const
	{
		Quaternion rotate = mTargetOrient;
		if( !mbFinish )
			rotate = Quaternion::nlerp(mfCurtime,mSrcOrient,mTargetOrient);
		return rotate;
	}

	void OrientationController::onFinish()
	{
		mbFinish = true;
		mfLength = 0.0f;
		mfCurtime = 0.0f;
		mParent->notifyFinish();
	}

	void OrientationController::update(Real timedelta)
	{
		if( mbFinish )	return;

		mfCurtime += timedelta;

		if( mfCurtime >= mfLength )
			onFinish();
	}

	void OrientationController::setFinished(bool bFinish)
	{
		mbFinish = bFinish;
		if( mbFinish ) onFinish();
	}

	//void OrientationController::setLength(Real fLength)
	//{
	//	this->mfLength = fLength;
	//}

	void OrientationController::setSrcAndDesDir(const Quaternion &src, const Vector3& dir)
	{
		mSrcOrient = src;
		Vector3 targetdir = dir;
		targetdir.normalise();
		Vector3 srcdir = mSrcOrient * Vector3::UNIT_X;
		Ogre::Quaternion quat;
		if ((1.0f + srcdir.dotProduct(targetdir)) < 0.0001f) 
		{	
			quat.FromAngleAxis(Ogre::Radian(180.0f),Vector3::UNIT_Y);
		}
		else
		{
			quat = srcdir.getRotationTo(targetdir);
		} // else
		quat.normalise();
		mTargetOrient = quat * src;
	}

	void OrientationController::onStart()
	{
		mbFinish = false;	
		mfCurtime = 0.0f;
	}

}